# -*- coding: utf-8 -*-
"""
Created on Mon Jul  6 00:47:01 2020

@author: Erfan
"""
from __future__ import print_function, division # Grab some 
import time
import numpy as np
from scipy import linalg, integrate, io
import matplotlib.pyplot as plt
from casadi import *
import random
from Model import *
from Parameters import *
from linearizing_matrixA import *
from scipy.linalg import expm

Nr,Nt,Nz,dr,dt,dz,Np,NN,Nx,Nw,Ny,Nv=circular_parameters()
DeltaT,Nsim=time_parameters()

#Irrigation amount
uu=np.zeros(Nsim)
uu[0:40]=-5.0e-06
uu[120:160]=-5.0e-06
uu[240:280]=-5.0e-06
uu[360:400]=-5.0e-06
uu[480:520]=-5.0e-06
uu[600:640]=-5.0e-06
uu[720:760]=-5.0e-06
uu[840:880]=-5.0e-06
uu[960:1000]=-5.0e-06
uu[1080:1120]=-5.0e-06


#Initial Conditions
x0=np.random.uniform(-1.75,-1.5, Nx)

# Process Noise
ww=0*np.random.randn(Nsim,Nw) ## If no noise is to be considered the coefficient is made 0
ww_1=0*np.random.randn(Nsim,Nw) 
#ww=5e-06*np.random.randn(Nsim,Nw) ##Process Noise
#ww_1=0*np.random.randn(Nsim,Nw) ##Process Noise


# Crop Model
Kc=np.zeros(Nsim)
Et=np.zeros(Nsim)


#Defining the arrays to contain the results
xx = np.zeros((Nsim+1,Nx))
xx[0, :] = x0


## Creating the Casadi Integrator
x_symbol = SX.sym('X',Nx) # The states
u_symbol=SX.sym('U',1)# The input which is the irrigation amount
w_symbol=SX.sym('W',Nw)# The disturbance
i_symbol=SX.sym('i_symbol',1)
Kc_symbol=SX.sym('Kc_symbol',1)
Et0_symbol=SX.sym('Et0_symbol',1)

F_symbol = ode_CP(x_symbol,u_symbol,w_symbol,i_symbol,Kc_symbol,Et0_symbol)

ODE = {'x': x_symbol, 'p': vertcat(u_symbol,w_symbol,i_symbol,Kc_symbol,Et0_symbol), 'ode': F_symbol}
opts = {'tf': DeltaT, 'regularity_check':True}  # seconds
I = integrator('I', 'cvodes', ODE, opts)  # Build casadi integrator


ph=np.zeros((Nx,Nsim))

for i in range(1, Nsim+1):
    time1 = time.time()
    print(i)
    d=i-1
    
    
    Ik = I(x0=xx[i-1], p=vertcat(uu[i-1],ww[i-1],d,Kc[i-1],Et[i-1]))# Model Simulation
    xk = Ik['xf'].full().ravel() 
    xx[i,:]=xk
    

    x_pre=xx[i-1,:]
    u_pre=uu[i-1]
    lin_A = linear_calculation(x_pre,u_pre,ww[i-1],i-1,Kc[i-1],Et[i-1])
        
    #Obtaining Degree of Observability Using Modal Degree of Observability
        
    A_c=DeltaT*lin_A
    a_c=np.array(A_c)
    a_d=scipy.linalg.expm(a_c)
    
    
    [e_Am,v]=np.linalg.eig(a_d)
    
    for ii in range(0, Nx):
       for jj in range(0, Nx):
           if np.abs((v[ii,jj]==0)):
               ph[ii] = 0
               print('the %d mode is not observable from node %d \n',jj,ii)
               break
           else:
               
               ph[ii,i-1] = ph[ii,i-1]+(1-(np.abs(e_Am[jj]))**2)*np.abs(v[ii,jj])**2    
      
    time2 = time.time()-time1
    print('Each step including everything',time2,'seconds to solve')
    
    

average_ph = np.zeros(Nx)  
for i in range(0,Nx):
    average_ph[i]  = np.mean(ph[i,:])
    
np.savetxt('degree.txt',ph)
np.savetxt('average_degree.txt',average_ph)
